#include "transform.h"

namespace venus {
void Transform::Print(FILE *f) const {
  m.Print(f);
}

Transform Translate(const Vector3 &delta) {
  Matrix4x4 m(1, 0, 0, delta.x,
              0, 1, 0, delta.y,
              0, 0, 1, delta.z,
              0, 0, 0,       1);
  Matrix4x4 minv(1, 0, 0, -delta.x,
                 0, 1, 0, -delta.y,
                 0, 0, 1, -delta.z,
                 0, 0, 0,        1);
  return Transform(m, minv);
}


Transform Scale(float x, float y, float z) {
  Matrix4x4 m(x, 0, 0, 0,
              0, y, 0, 0,
              0, 0, z, 0,
              0, 0, 0, 1);
  Matrix4x4 minv(1.f/x,     0,     0,     0,
                 0,     1.f/y,     0,     0,
                 0,         0,     1.f/z, 0,
                 0,         0,     0,     1);
  return Transform(m, minv);
}


Transform RotateX(float angle) {
  float sin_t = sinf(Math::deg2rad(angle));
  float cos_t = cosf(Math::deg2rad(angle));
  Matrix4x4 m(1,     0,      0, 0,
              0, cos_t, -sin_t, 0,
              0, sin_t,  cos_t, 0,
              0,     0,      0, 1);
  return Transform(m, Transpose(m));
}


Transform RotateY(float angle) {
  float sin_t = sinf(Math::deg2rad(angle));
  float cos_t = cosf(Math::deg2rad(angle));
  Matrix4x4 m( cos_t,   0,  sin_t, 0,
               0,   1,      0, 0,
              -sin_t,   0,  cos_t, 0,
               0,   0,   0,    1);
  return Transform(m, Transpose(m));
}



Transform RotateZ(float angle) {
  float sin_t = sinf(Math::deg2rad(angle));
  float cos_t = cosf(Math::deg2rad(angle));
  Matrix4x4 m(cos_t, -sin_t, 0, 0,
              sin_t,  cos_t, 0, 0,
              0,      0, 1, 0,
              0,      0, 0, 1);
  return Transform(m, Transpose(m));
}


Transform Rotate(float angle, const Vector3 &axis) {
  Vector3 a = Normalize(axis);
  float s = sinf(Math::deg2rad(angle));
  float c = cosf(Math::deg2rad(angle));
  float m[4][4];

  m[0][0] = a.x * a.x + (1.f - a.x * a.x) * c;
  m[0][1] = a.x * a.y * (1.f - c) - a.z * s;
  m[0][2] = a.x * a.z * (1.f - c) + a.y * s;
  m[0][3] = 0;

  m[1][0] = a.x * a.y * (1.f - c) + a.z * s;
  m[1][1] = a.y * a.y + (1.f - a.y * a.y) * c;
  m[1][2] = a.y * a.z * (1.f - c) - a.x * s;
  m[1][3] = 0;

  m[2][0] = a.x * a.z * (1.f - c) - a.y * s;
  m[2][1] = a.y * a.z * (1.f - c) + a.x * s;
  m[2][2] = a.z * a.z + (1.f - a.z * a.z) * c;
  m[2][3] = 0;

  m[3][0] = 0;
  m[3][1] = 0;
  m[3][2] = 0;
  m[3][3] = 1;

  Matrix4x4 mat(m);
  return Transform(mat, Transpose(mat));
}


Transform LookAt(const Point &pos, const Point &look, const Vector3 &up) {
  float m[4][4];
  // Initialize fourth column of viewing matrix
  m[0][3] = pos.x;
  m[1][3] = pos.y;
  m[2][3] = pos.z;
  m[3][3] = 1;

  // Initialize first three columns of viewing matrix
  Vector3 dir = Normalize(look - pos);
  Vector3 left = Normalize(Cross(Normalize(up), dir));
  Vector3 newUp = Cross(dir, left);
  m[0][0] = left.x;
  m[1][0] = left.y;
  m[2][0] = left.z;
  m[3][0] = 0.;
  m[0][1] = newUp.x;
  m[1][1] = newUp.y;
  m[2][1] = newUp.z;
  m[3][1] = 0.;
  m[0][2] = dir.x;
  m[1][2] = dir.y;
  m[2][2] = dir.z;
  m[3][2] = 0.;
  Matrix4x4 camToWorld(m);
  return Transform(Inverse(camToWorld), camToWorld);
}
AABB Transform::Do(const AABB &b) const {
  const Transform &M = *this;
  AABB ret(M.Do(Point(b.minP.x, b.minP.y, b.minP.z)));
  ret.Union(M.Do(Point(b.maxP.x, b.minP.y, b.minP.z)));
  ret.Union(M.Do(Point(b.minP.x, b.maxP.y, b.minP.z)));
  ret.Union(M.Do(Point(b.minP.x, b.minP.y, b.maxP.z)));
  ret.Union(M.Do(Point(b.minP.x, b.maxP.y, b.maxP.z)));
  ret.Union(M.Do(Point(b.maxP.x, b.maxP.y, b.minP.z)));
  ret.Union(M.Do(Point(b.maxP.x, b.minP.y, b.maxP.z)));
  ret.Union(M.Do(Point(b.maxP.x, b.maxP.y, b.maxP.z)));
  return ret;
}
Transform Transform::operator*(const Transform &t2) const {
  Matrix4x4 m1 = Matrix4x4::Mul(m, t2.m);
  Matrix4x4 m2 = Matrix4x4::Mul(t2.mInv, mInv);
  return Transform(m1, m2);
}
bool Transform::SwapsHandedness() const {
  float det = ((m.m[0][0] * (m.m[1][1] * m.m[2][2] - m.m[1][2] * m.m[2][1])) -
               (m.m[0][1] * (m.m[1][0] * m.m[2][2] - m.m[1][2] * m.m[2][0])) +
               (m.m[0][2] * (m.m[1][0] * m.m[2][1] - m.m[1][1] * m.m[2][0])));
  return det < 0.f;
}
Transform Orthographic(float znear, float zfar) {
  return Scale(1.f, 1.f, 1.f / (zfar-znear)) *
         Translate(Vector3(0.f, 0.f, -znear));
}
Transform Perspective(float fov, float n, float f) {
  // Perform projective divide
  Matrix4x4 persp = Matrix4x4(1, 0,           0,              0,
                              0, 1,           0,              0,
                              0, 0, f / (f - n), -f*n / (f - n),
                              0, 0,           1,              0);

  // Scale to canonical viewing volume
  float invTanAng = 1.f / tanf(Math::deg2rad(fov) / 2.f);
  return Scale(invTanAng, invTanAng, 1) * Transform(persp);
}

} // namespace venus
